/******************************************************************************
 * Copyright (C) Ultraleap, Inc. 2011-2021.                                   *
 *                                                                            *
 * Use subject to the terms of the Apache License 2.0 available at            *
 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement           *
 * between Ultraleap and you, your company or other organization.             *
 ******************************************************************************/

using Leap.Interaction.Internal.InteractionEngineUtility;
using Leap.Unity.Attributes;
using Leap.Unity.RuntimeGizmos;
using System;
using System.Collections.Generic;
using UnityEngine;

#if UNITY_EDITOR
using UnityEditor;
#endif

namespace Leap.Unity.Interaction
{

    [DisallowMultipleComponent]
    [ExecuteInEditMode]
    public class InteractionManager : MonoBehaviour, IInternalInteractionManager,
                                                     IRuntimeGizmoComponent
    {

        #region Inspector

        // Header "Interaction Controllers" via InteractionManagerEditor.cs.
        [SerializeField]
        private InteractionControllerSet _interactionControllers = new InteractionControllerSet();
        /// <summary>
        /// Gets the list of interaction controllers managed by this InteractionManager.
        /// </summary>
        public ReadonlyHashSet<InteractionController> interactionControllers
        {
            get { return _interactionControllers; }
        }

        [Header("Interaction Settings")]

        [SerializeField]
        [Tooltip("Beyond this radius, an interaction object will not receive hover or primary "
               + "hover callbacks from an interaction controller. (Smaller values are "
               + "cheaper.) This value is automatically scaled under the hood by the "
               + "Interaction Manager's lossyScale.x, so it's recommended to keep your "
               + "Interaction Manager with unit scale underneath your 'Player' Transform if "
               + "you expect your player's hands or controllers to ever have non-unit scale.")]
        public float hoverActivationRadius = 0.2F;

        [Tooltip("Beyond this radius, an interaction object will not be considered for "
               + "contact or grasping logic. The radius should be small as an optimization "
               + "but certainly not smaller than an interaction controller and not too tight "
               + "around the controller to allow good behavior when it is moving quickly "
               + "through space. This value is automatically scaled under the hood by the "
               + "Interaction Manager's lossyScale.x, so it's recommended to keep your "
               + "Interaction Manager with unit scale underneath your 'Player' Transform if "
               + "you expect your player's hands or controllers to ever have non-unit scale.")]
        public float touchActivationRadius = 0.075F;

        public enum MultiGraspHoldingMode
        {
            PreservePosePerController,
            ReinitializeOnAnyRelease
        }
        [Tooltip("If set to PreservePosePerController, when holding a multi-grasp-enabled object and releasing with a single hand or controller, the object's held pose will adjust to reflect only the remaining holding controllers.\n\nIf set to PreservePosePerController, when any hand or controller releases an object, the remaining controllers will reinitialize their holding pose to match the last-held state of the object, allowing the user to rotate and translate the object more loosely.\n\nPreservePosePerController is the default setting.")]
        public MultiGraspHoldingMode multiGraspHoldingMode = MultiGraspHoldingMode.PreservePosePerController;

        [Header("Layer Settings")]
        [Tooltip("Whether or not to create the layers used for interaction when the scene "
               + "runs. Interactions require an interaction layer (for objects), a grasped "
               + "object layer, and a contact bone layer (for interaction controller 'bone' "
               + "colliders). Keep this checked to have these layers created for you, but be "
               + "aware that the generated layers will have blank names due to Unity "
               + "limitations.")]
        [SerializeField]
        [EditTimeOnly]
        protected bool _autoGenerateLayers = true;
        /// <summary>
        /// Gets whether auto-generate layers was enabled for this Interaction Manager.
        /// </summary>
        public bool autoGenerateLayers { get { return _autoGenerateLayers; } }

        [Tooltip("When automatically generating layers, the Interaction layer (for "
               + "interactable objects) will use the same physics collision flags as the "
               + "layer specified here.")]
        [SerializeField]
        protected SingleLayer _templateLayer = 0;
        public SingleLayer templateLayer { get { return _templateLayer; } }

        [Tooltip("The layer for interactable objects (i.e. InteractionBehaviours). Usually "
               + "this would have the same collision flags as the Default layer, but it "
               + "should be its own layer so interaction controllers don't have to check "
               + "collision against all physics objects in the scene.")]
        [SerializeField]
        protected SingleLayer _interactionLayer = 0;
        public SingleLayer interactionLayer { get { return _interactionLayer; } }

        [Tooltip("The layer objects are moved to when they become grasped, or if they are "
               + "otherwise ignoring controller contact. This layer should not collide with "
               + "the contact bone layer, but should collide with everything else that the "
               + "interaction layer collides with.")]
        [SerializeField]
        protected SingleLayer _interactionNoContactLayer = 0;
        public SingleLayer interactionNoContactLayer { get { return _interactionNoContactLayer; } }

        [Tooltip("The layer containing the collider 'bones' of the interaction controller. "
               + "This layer should collide with anything you'd like to be able to touch, "
               + "but it should not collide with the grasped object layer.")]
        [SerializeField]
        protected SingleLayer _contactBoneLayer = 0;
        public SingleLayer contactBoneLayer { get { return _contactBoneLayer; } }

        [Header("Debug Settings")]
        [SerializeField]
        [Tooltip("Rendering runtime gizmos requires having a Runtime Gizmo Manager somewhere "
               + "in the scene.")]
        private bool _drawControllerRuntimeGizmos = false;

        #endregion

        #region Events

        public Action OnGraphicalUpdate = () => { };
        public Action OnPrePhysicalUpdate = () => { };
        public Action OnPostPhysicalUpdate = () => { };

        #endregion

        #region Scale Support

        private float _scale = 1F;

        /// <summary>
        /// Interaction objects further than this distance from a given controller's hover
        /// point will not be considered for any hover interactions with that controller.
        /// </summary>
        public float WorldHoverActivationRadius { get { return hoverActivationRadius * _scale; } }

        /// <summary>
        /// Interaction objects further than this distance from a given controller's hover
        /// point will not be considered for any contact or grasping interactions with that
        /// controller.
        /// </summary>
        public float WorldTouchActivationRadius { get { return touchActivationRadius * _scale; } }

        /// <summary>
        /// A scale that can be used to appropriately transform distances that otherwise expect
        /// one Unity unit to correspond to one meter.
        /// </summary>
        public float SimulationScale { get { return _scale; } }

        #endregion

        #region Object Tracking

        private HashSet<IInteractionBehaviour> _interactionObjects = new HashSet<IInteractionBehaviour>();
        /// <summary>
        /// Gets a set of all interaction objects currently registered with this Interaction
        /// Manager.
        /// </summary>
        public ReadonlyHashSet<IInteractionBehaviour> interactionObjects
        {
            get { return _interactionObjects; }
        }

        private Dictionary<Rigidbody, IInteractionBehaviour> _interactionObjectBodies;
        /// <summary>
        /// Maps a Rigidbody to its attached interaction object, if the Rigidbody is part of
        /// and interaction object.
        /// </summary>
        public Dictionary<Rigidbody, IInteractionBehaviour> interactionObjectBodies
        {
            get
            {
                if (_interactionObjectBodies == null)
                {
                    _interactionObjectBodies = new Dictionary<Rigidbody, IInteractionBehaviour>();
                }
                return _interactionObjectBodies;
            }
        }

        private Dictionary<Rigidbody, ContactBone> _contactBoneBodies;
        /// <summary>
        /// Maps a Rigidbody to its attached ContactBone, if the Rigidbody is part of an
        /// interaction controller.
        /// </summary>
        public Dictionary<Rigidbody, ContactBone> contactBoneBodies
        {
            get
            {
                if (_contactBoneBodies == null)
                {
                    _contactBoneBodies = new Dictionary<Rigidbody, ContactBone>();
                }
                return _contactBoneBodies;
            }
        }

        #endregion

        #region Singleton Pattern (Optional)

        private static InteractionManager s_instance;
        /// <summary> Often, only one InteractionManager is necessary per Unity scene.
        /// This property will contain that InteractionManager as soon as its Awake()
        /// method is called. Using more than one InteractionManager is valid, but be
        /// sure to assign any InteractionBehaviour's desired manager appropriately.
        /// </summary>
        ///
        /// <remarks> By default, this static property contains the first InteractionManager
        /// that has had its Awake() method called in the current scene. If an
        /// InteractionBehaviourBase does not have a non-null interactionManager by the
        /// time it has Start() called, it will default to using the InteractionManager
        /// referenced here.
        ///
        /// If you have multiple InteractionManagers in your scene, you should be sure to
        /// assign InteractionBehaviours' managers appropriately. If you instantiate an
        /// InteractionBehaviour at runtime, you should assign its InteractionManager
        /// right after you instantiate it. </remarks>
        public static InteractionManager instance
        {
            get
            {
                if (s_instance == null) { s_instance = FindObjectOfType<InteractionManager>(); }
                return s_instance;
            }
            set { s_instance = value; }
        }

        #endregion

        #region Unity Events

        void OnValidate()
        {
            if (!Application.isPlaying && _autoGenerateLayers)
            {
                generateAutomaticLayers();
            }

            refreshInteractionControllers();
        }

        void Awake()
        {
            refreshInteractionControllers();

            if (!Application.isPlaying) return;

            if (s_instance == null) s_instance = this;

            if (_autoGenerateLayers)
            {
                generateAutomaticLayers();
                setupAutomaticCollisionLayers();
            }

            _prevPosition = this.transform.position;
            _prevRotation = this.transform.rotation;

#if UNITY_EDITOR
            if (_drawControllerRuntimeGizmos == true)
            {
                if (FindObjectOfType<RuntimeGizmoManager>() == null)
                {
                    Debug.LogWarning("'_drawControllerRuntimeGizmos' is enabled, but there is no "
                                   + "RuntimeGizmoManager in your scene. Please add one if you'd "
                                   + "like to render gizmos in the editor and in your headset.");
                }
            }
#endif
        }

        void OnDisable()
        {
#if UNITY_EDITOR
            if (!Application.isPlaying) return;
#endif

            foreach (var intController in _interactionControllers)
            {
                // Disables the colliders in the interaction controller;
                // soft contact won't be applied if the controller is not updating.
                intController.EnableSoftContact();

                if (intController.isGraspingObject)
                {
                    intController.ReleaseGrasp();
                }
            }
        }

        void Update()
        {
#if UNITY_EDITOR
            refreshInteractionControllers();
#endif
        }

        void FixedUpdate()
        {
            OnPrePhysicalUpdate();

            // Physics should only be synced once at the beginning of the physics simulation.
            // (Will be re-set to its original value at the end of the update.)
#if UNITY_2017_2_OR_NEWER
            var preUpdateAutoSyncTransforms = Physics.autoSyncTransforms;
            Physics.autoSyncTransforms = false;
#endif
            try
            {

                refreshInteractionControllers();

#if UNITY_EDITOR
                if (!Application.isPlaying) return;
#endif

                using (new ProfilerSample("Interaction Manager FixedUpdate", this.gameObject))
                {
                    // Ensure scale information is up-to-date.
                    _scale = this.transform.lossyScale.x;

                    // Update each interaction controller (Leap hands or supported VR controllers).
                    fixedUpdateInteractionControllers();

                    // Perform each interaction object's FixedUpdateObject.
                    using (new ProfilerSample("FixedUpdateObject per-InteractionBehaviour"))
                    {
                        foreach (var interactionObj in _interactionObjects)
                        {
                            interactionObj.FixedUpdateObject();
                        }
                    }

                    // Apply soft contacts from all controllers in a unified solve.
                    // (This will clear softContacts and originalVelocities as well.)
                    using (new ProfilerSample("Apply Soft Contacts"))
                    {
                        if (_drawControllerRuntimeGizmos)
                        {
                            _softContactsToDraw = new List<PhysicsUtility.SoftContact>(_softContacts);
                        }
                        if (_softContacts.Count > 0)
                        {
                            PhysicsUtility.applySoftContacts(_softContacts, _softContactOriginalVelocities);
                        }
                    }
                }

                OnPostPhysicalUpdate();

                updateMovingFrameOfReferenceSupport();

                if (autoGenerateLayers)
                {
                    autoUpdateContactBoneLayerCollision();
                }

            }
            finally
            {
#if UNITY_2017_2_OR_NEWER
                // Restore the autoSyncTransforms setting to whatever the user had it as before
                // the Manager FixedUpdate.
                Physics.autoSyncTransforms = preUpdateAutoSyncTransforms;
#endif
            }
        }

        void LateUpdate()
        {
            OnGraphicalUpdate();
        }

        #endregion

        #region Controller Interaction State & Callbacks Update

        private HashSet<InteractionController> _activeControllersBuffer = new HashSet<InteractionController>();
        private HashSet<InteractionController> _hoverControllersBuffer = new HashSet<InteractionController>();
        private HashSet<InteractionController> _contactControllersBuffer = new HashSet<InteractionController>();
        private HashSet<InteractionController> _graspingControllersBuffer = new HashSet<InteractionController>();

        private void fixedUpdateInteractionControllers()
        {

            _hoverControllersBuffer.Clear();
            _contactControllersBuffer.Clear();
            _graspingControllersBuffer.Clear();
            _activeControllersBuffer.Clear();
            foreach (var controller in interactionControllers)
            {
                if (!controller.isActiveAndEnabled) continue;

                _activeControllersBuffer.Add(controller);

                if (controller.hoverEnabled) _hoverControllersBuffer.Add(controller);
                if (controller.contactEnabled) _contactControllersBuffer.Add(controller);
                if (controller.graspingEnabled) _graspingControllersBuffer.Add(controller);
            }

            using (new ProfilerSample("Fixed Update Controllers (General Update)"))
            {
                // Perform general controller update, for controller collider and point
                // representations.
                foreach (var controller in _activeControllersBuffer)
                {
                    if (!controller.isActiveAndEnabled) continue;
                    (controller as IInternalInteractionController).FixedUpdateController();
                }
            }

            using (new ProfilerSample("Fixed Update Controllers (Interaction State and Callbacks)"))
            {

                /*
                  * Interactions are checked here in a very specific manner so that interaction
                  * callbacks always occur in a strict order and interaction object state is
                  * always updated directly before the relevant callbacks occur.
                  *
                  * Interaction callbacks will only occur outside this order if a script
                  * manually forces interaction state-changes; for example, calling
                  * interactionController.ReleaseGrasp() will immediately call
                  * interactionObject.OnPerControllerGraspEnd() on the formerly grasped object.
                  *
                  * Callback order:
                  * - Suspension (when a grasped object's grasping controller loses tracking)
                  * - Just-Ended Interactions (Grasps, then Contacts, then Hovers)
                  * - Just-Begun Interactions (Hovers, then Contacts, then Grasps)
                  * - Sustained Interactions (Hovers, then Contacts, then Grasps)
                  */

                // Suspension //

                // Check controllers beginning object suspension.
                foreach (var controller in _graspingControllersBuffer)
                {
                    IInteractionBehaviour suspendedObj;
                    if ((controller as IInternalInteractionController).CheckSuspensionBegin(out suspendedObj))
                    {
                        suspendedObj.BeginSuspension(controller);
                    }
                }

                // Check controllers ending object suspension.
                foreach (var controller in _graspingControllersBuffer)
                {
                    IInteractionBehaviour resumedObj;
                    if ((controller as IInternalInteractionController).CheckSuspensionEnd(out resumedObj))
                    {
                        resumedObj.EndSuspension(controller);
                    }
                }

                // Ending Interactions //

                checkEndingGrasps(_graspingControllersBuffer);
                checkEndingContacts(_contactControllersBuffer);
                checkEndingPrimaryHovers(_hoverControllersBuffer);
                checkEndingHovers(_hoverControllersBuffer);

                // Beginning Interactions //

                checkBeginningHovers(_hoverControllersBuffer);
                checkBeginningPrimaryHovers(_hoverControllersBuffer);
                checkBeginningContacts(_contactControllersBuffer);
                checkBeginningGrasps(_graspingControllersBuffer);

                // Sustained Interactions //

                checkSustainingHovers(_hoverControllersBuffer);
                checkSustainingPrimaryHovers(_hoverControllersBuffer);
                checkSustainingContacts(_contactControllersBuffer);
                checkSustainingGrasps(_graspingControllersBuffer);

            }
        }

        #region State-Check Remapping Functions

        private void checkEndingGrasps(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapInteractionObjectStateChecks(
              controllers: interactionControllers,
              stateCheckFunc: (InteractionController maybeReleasingController, out IInteractionBehaviour maybeReleasedObject) =>
              {
                  return (maybeReleasingController as IInternalInteractionController).CheckGraspEnd(out maybeReleasedObject);
              },
              actionPerInteractionObject: (releasedObject, releasingIntControllers) =>
              {
                  releasedObject.EndGrasp(releasingIntControllers);
              });
        }

        private void checkEndingContacts(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapMultiInteractionObjectStateChecks(
              controllers: interactionControllers,
              multiObjectStateCheckFunc: (InteractionController maybeEndedContactingController, out HashSet<IInteractionBehaviour> endContactedObjects) =>
              {
                  return (maybeEndedContactingController as IInternalInteractionController).CheckContactEnd(out endContactedObjects);
              },
              actionPerInteractionObject: (endContactedObject, endContactedIntControllers) =>
              {
                  endContactedObject.EndContact(endContactedIntControllers);
              });
        }

        private void checkEndingPrimaryHovers(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapInteractionObjectStateChecks(
              controllers: interactionControllers,
              stateCheckFunc: (InteractionController maybeEndedPrimaryHoveringController, out IInteractionBehaviour endPrimaryHoveredObject) =>
              {
                  return (maybeEndedPrimaryHoveringController as IInternalInteractionController).CheckPrimaryHoverEnd(out endPrimaryHoveredObject);
              },
              actionPerInteractionObject: (endPrimaryHoveredObject, noLongerPrimaryHoveringControllers) =>
              {
                  endPrimaryHoveredObject.EndPrimaryHover(noLongerPrimaryHoveringControllers);
              });
        }

        private void checkEndingHovers(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapMultiInteractionObjectStateChecks(
              controllers: interactionControllers,
              multiObjectStateCheckFunc: (InteractionController maybeEndedHoveringController, out HashSet<IInteractionBehaviour> endHoveredObjects) =>
              {
                  return (maybeEndedHoveringController as IInternalInteractionController).CheckHoverEnd(out endHoveredObjects);
              },
              actionPerInteractionObject: (endHoveredObject, endHoveringIntControllers) =>
              {
                  endHoveredObject.EndHover(endHoveringIntControllers);
              });
        }

        private void checkBeginningHovers(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapMultiInteractionObjectStateChecks(
              controllers: interactionControllers,
              multiObjectStateCheckFunc: (InteractionController maybeBeganHoveringController, out HashSet<IInteractionBehaviour> beganHoveredObjects) =>
              {
                  return (maybeBeganHoveringController as IInternalInteractionController).CheckHoverBegin(out beganHoveredObjects);
              },
              actionPerInteractionObject: (beganHoveredObject, beganHoveringIntControllers) =>
              {
                  beganHoveredObject.BeginHover(beganHoveringIntControllers);
              });
        }

        private void checkBeginningPrimaryHovers(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapInteractionObjectStateChecks(
              controllers: interactionControllers,
              stateCheckFunc: (InteractionController maybeBeganPrimaryHoveringController, out IInteractionBehaviour primaryHoveredObject) =>
              {
                  return (maybeBeganPrimaryHoveringController as IInternalInteractionController).CheckPrimaryHoverBegin(out primaryHoveredObject);
              },
              actionPerInteractionObject: (newlyPrimaryHoveredObject, beganPrimaryHoveringControllers) =>
              {
                  newlyPrimaryHoveredObject.BeginPrimaryHover(beganPrimaryHoveringControllers);
              });
        }

        private void checkBeginningContacts(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapMultiInteractionObjectStateChecks(
              controllers: interactionControllers,
              multiObjectStateCheckFunc: (InteractionController maybeBeganContactingController, out HashSet<IInteractionBehaviour> beganContactedObjects) =>
              {
                  return (maybeBeganContactingController as IInternalInteractionController).CheckContactBegin(out beganContactedObjects);
              },
              actionPerInteractionObject: (beganContactedObject, beganContactingIntControllers) =>
              {
                  beganContactedObject.BeginContact(beganContactingIntControllers);
              });
        }

        private void checkBeginningGrasps(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapInteractionObjectStateChecks(
              controllers: interactionControllers,
              stateCheckFunc: (InteractionController maybeBeganGraspingController, out IInteractionBehaviour graspedObject) =>
              {
                  return (maybeBeganGraspingController as IInternalInteractionController).CheckGraspBegin(out graspedObject);
              },
              actionPerInteractionObject: (newlyGraspedObject, beganGraspingIntControllers) =>
              {
                  newlyGraspedObject.BeginGrasp(beganGraspingIntControllers);
              });
        }

        private void checkSustainingHovers(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapMultiInteractionObjectStateChecks(
              controllers: interactionControllers,
              multiObjectStateCheckFunc: (InteractionController maybeSustainedHoveringController, out HashSet<IInteractionBehaviour> hoveredObjects) =>
              {
                  return (maybeSustainedHoveringController as IInternalInteractionController).CheckHoverStay(out hoveredObjects);
              },
              actionPerInteractionObject: (hoveredObject, hoveringIntControllers) =>
              {
                  hoveredObject.StayHovered(hoveringIntControllers);
              });
        }

        private void checkSustainingPrimaryHovers(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapInteractionObjectStateChecks(
              controllers: interactionControllers,
              stateCheckFunc: (InteractionController maybeSustainedPrimaryHoveringController, out IInteractionBehaviour primaryHoveredObject) =>
              {
                  return (maybeSustainedPrimaryHoveringController as IInternalInteractionController).CheckPrimaryHoverStay(out primaryHoveredObject);
              },
              actionPerInteractionObject: (primaryHoveredObject, primaryHoveringControllers) =>
              {
                  primaryHoveredObject.StayPrimaryHovered(primaryHoveringControllers);
              });
        }

        private void checkSustainingContacts(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapMultiInteractionObjectStateChecks(
              controllers: interactionControllers,
              multiObjectStateCheckFunc: (InteractionController maybeSustainedContactingController, out HashSet<IInteractionBehaviour> contactedObjects) =>
              {
                  return (maybeSustainedContactingController as IInternalInteractionController).CheckContactStay(out contactedObjects);
              },
              actionPerInteractionObject: (contactedObject, contactingIntControllers) =>
              {
                  contactedObject.StayContacted(contactingIntControllers);
              });
        }

        private void checkSustainingGrasps(ReadonlyHashSet<InteractionController> interactionControllers)
        {
            remapInteractionObjectStateChecks(
              controllers: interactionControllers,
              stateCheckFunc: (InteractionController maybeSustainedGraspingController, out IInteractionBehaviour graspedObject) =>
              {
                  return (maybeSustainedGraspingController as IInternalInteractionController).CheckGraspHold(out graspedObject);
              },
              actionPerInteractionObject: (contactedObject, contactingIntControllers) =>
              {
                  contactedObject.StayGrasped(contactingIntControllers);
              });
        }

        private delegate bool StateChangeCheckFunc(InteractionController controller, out IInteractionBehaviour obj);
        private delegate bool MultiStateChangeCheckFunc(InteractionController controller, out HashSet<IInteractionBehaviour> objs);

        [ThreadStatic]
        private static Dictionary<IInteractionBehaviour, List<InteractionController>> s_objControllersMap = new Dictionary<IInteractionBehaviour, List<InteractionController>>();

        /// <summary>
        /// Checks object state per-controller, then calls an action per-object with all controller checks that reported back an object.
        /// </summary>
        private void remapInteractionObjectStateChecks(ReadonlyHashSet<InteractionController> controllers,
                                                       StateChangeCheckFunc stateCheckFunc,
                                                       Action<IInteractionBehaviour, List<InteractionController>> actionPerInteractionObject)
        {

            // Ensure the object->controllers buffer is non-null (ThreadStatic quirk) and clean.
            if (s_objControllersMap == null) s_objControllersMap = new Dictionary<IInteractionBehaviour, List<InteractionController>>();
            s_objControllersMap.Clear();

            // In a nutshell, this remaps methods per-controller that output an interaction object if the controller changed that object's state
            // to methods per-object with all of the controllers for which the check produced a state-change.
            foreach (var controller in controllers)
            {
                IInteractionBehaviour objectWhoseStateChanged;
                if (stateCheckFunc(controller, out objectWhoseStateChanged))
                {
                    if (!s_objControllersMap.ContainsKey(objectWhoseStateChanged))
                    {
                        s_objControllersMap[objectWhoseStateChanged] = Pool<List<InteractionController>>.Spawn();
                    }
                    s_objControllersMap[objectWhoseStateChanged].Add(controller);
                }
            }
            // Finally, iterate through each (object, controllers) pair and call the action for each pair
            foreach (var objControllesPair in s_objControllersMap)
            {
                actionPerInteractionObject(objControllesPair.Key, objControllesPair.Value);

                // Clear each controllers list and return it to the list pool.
                objControllesPair.Value.Clear();
                Pool<List<InteractionController>>.Recycle(objControllesPair.Value);
            }
        }

        /// <summary>
        /// Checks object state per-controller, then calls an action per-object with all controller checks that reported back objects.
        /// </summary>
        private void remapMultiInteractionObjectStateChecks(ReadonlyHashSet<InteractionController> controllers,
                                                            MultiStateChangeCheckFunc multiObjectStateCheckFunc,
                                                            Action<IInteractionBehaviour, List<InteractionController>> actionPerInteractionObject)
        {
            // Ensure object<->controllers buffer is non-null (ThreadStatic quirk) and clean.
            if (s_objControllersMap == null) s_objControllersMap = new Dictionary<IInteractionBehaviour, List<InteractionController>>();
            s_objControllersMap.Clear();

            // In a nutshell, this remaps methods per-controller that output multiple interaction objects if the controller changed those objects' states
            // to methods per-object with all of the controllers for which the check produced a state-change.
            foreach (var controller in controllers)
            {
                HashSet<IInteractionBehaviour> stateChangedObjects;
                if (multiObjectStateCheckFunc(controller, out stateChangedObjects))
                {
                    foreach (var stateChangedObject in stateChangedObjects)
                    {
                        if (!s_objControllersMap.ContainsKey(stateChangedObject))
                        {
                            s_objControllersMap[stateChangedObject] = Pool<List<InteractionController>>.Spawn();
                        }
                        s_objControllersMap[stateChangedObject].Add(controller);
                    }
                }
            }
            // Finally, iterate through each (object, controllers) pair and call the action for each pair
            foreach (var objControllersPair in s_objControllersMap)
            {
                actionPerInteractionObject(objControllersPair.Key, objControllersPair.Value);

                // Clear each controllers list and return it to the list pool.
                objControllersPair.Value.Clear();
                Pool<List<InteractionController>>.Recycle(objControllersPair.Value);
            }
        }

        #endregion

        #region State Notifications

        // TODO: Delete this whole sction

        //private HashSet<InteractionController> controllerSetBuffer = new HashSet<InteractionController>();

        //void IInternalInteractionManager.NotifyControllerDisabled(InteractionController controller) {
        //  controllerSetBuffer.Clear();
        //  controllerSetBuffer.Add(controller);

        //  checkEndingGrasps(controllerSetBuffer);
        //  checkEndingContacts(controllerSetBuffer);
        //  checkEndingPrimaryHovers(controllerSetBuffer);
        //  checkEndingHovers(controllerSetBuffer);
        //}

        //void IInternalInteractionManager.NotifyHoverDisabled(InteractionController controller) {
        //  controllerSetBuffer.Clear();
        //  controllerSetBuffer.Add(controller);

        //  checkEndingPrimaryHovers(controllerSetBuffer);
        //  checkEndingHovers(controllerSetBuffer);
        //}

        //void IInternalInteractionManager.NotifyContactDisabled(InteractionController controller) {
        //  controllerSetBuffer.Clear();
        //  controllerSetBuffer.Add(controller);

        //  checkEndingContacts(controllerSetBuffer);
        //}

        //void IInternalInteractionManager.NotifyObjectHoverIgnored(IInteractionBehaviour intObj) {
        //  controllerSetBuffer.Clear();

        //  foreach (var controller in interactionControllers) {
        //    if (controller.hoveredObjects.Contains(intObj)) {
        //      (controller as IInternalInteractionController).ClearHoverTrackingForObject(intObj);

        //      controllerSetBuffer.Add(controller);
        //    }
        //  }

        //  checkEndingHovers(controllerSetBuffer);
        //}

        //void IInternalInteractionManager.NotifyObjectPrimaryHoverIgnored(IInteractionBehaviour intObj) {
        //  controllerSetBuffer.Clear();

        //  foreach (var controller in interactionControllers) {
        //    if (controller.primaryHoveredObject == intObj) {
        //      (controller as IInternalInteractionController).ClearPrimaryHoverTrackingForObject(intObj);

        //      controllerSetBuffer.Add(controller);
        //    }
        //  }

        //  checkEndingPrimaryHovers(controllerSetBuffer);
        //}

        //void IInternalInteractionManager.NotifyObjectContactIgnored(IInteractionBehaviour intObj) {
        //  controllerSetBuffer.Clear();

        //  foreach (var controller in interactionControllers) {
        //    if (controller.contactingObjects.Contains(intObj)) {
        //      (controller as IInternalInteractionController).ClearContactTrackingForObject(intObj);

        //      controllerSetBuffer.Add(controller);
        //    }
        //  }

        //  checkEndingContacts(controllerSetBuffer);
        //}

        #endregion

        #endregion

        #region Object Registration

        public void RegisterInteractionBehaviour(IInteractionBehaviour interactionObj)
        {
            _interactionObjects.Add(interactionObj);
            interactionObjectBodies[interactionObj.rigidbody] = interactionObj;
        }

        /// <summary>
        /// Returns true if the Interaction Behaviour was registered with this manager;
        /// otherwise returns false. The manager is guaranteed not to have the Interaction
        /// Behaviour registered after calling this method.
        /// </summary>
        public bool UnregisterInteractionBehaviour(IInteractionBehaviour interactionObj)
        {
            bool wasRemovalSuccessful = _interactionObjects.Remove(interactionObj);
            if (wasRemovalSuccessful)
            {
                foreach (var intController in _interactionControllers)
                {
                    intController.ReleaseObject(interactionObj);

                    intController.NotifyObjectUnregistered(interactionObj);
                }
                interactionObjectBodies.Remove(interactionObj.rigidbody);
            }
            return wasRemovalSuccessful;
        }

        public bool IsBehaviourRegistered(IInteractionBehaviour interactionObj)
        {
            return _interactionObjects.Contains(interactionObj);
        }

        #endregion

        #region Moving Frame of Reference Support

        public bool hasMovingFrameOfReference
        {
            get
            {
                return (this.transform.position - _prevPosition).magnitude > 0.0001F
                    || Quaternion.Angle(transform.rotation * Quaternion.Inverse(_prevRotation),
                                        Quaternion.identity) > 0.01F;
            }
        }

        // Support for a moving frame of reference.
        private Vector3 _prevPosition = Vector3.zero;
        private Quaternion _prevRotation = Quaternion.identity;

        private void updateMovingFrameOfReferenceSupport()
        {
            _prevPosition = this.transform.position;
            _prevRotation = this.transform.rotation;
        }

        /// <summary>
        /// Transforms a position and rotation ahead by one FixedUpdate based on the prior
        /// motion of the InteractionManager.
        ///
        /// This method is used to support having the player in a moving frame of reference.
        /// </summary>
        public void TransformAheadByFixedUpdate(Vector3 position, Quaternion rotation, out Vector3 newPosition, out Quaternion newRotation)
        {
            Vector3 worldDisplacement = this.transform.position - _prevPosition;
            Quaternion worldRotation = this.transform.rotation * Quaternion.Inverse(_prevRotation);
            newPosition = ((worldRotation * (position - this.transform.position + worldDisplacement))) + this.transform.position;
            newRotation = worldRotation * rotation;
        }

        /// <summary>
        /// Transforms a position ahead by one FixedUpdate based on the prior motion (position
        /// AND rotation) of the InteractionManager.
        ///
        /// This method is used to support having the player in a moving frame of reference.
        /// </summary>
        public void TransformAheadByFixedUpdate(Vector3 position, out Vector3 newPosition)
        {
            Vector3 worldDisplacement = this.transform.position - _prevPosition;
            Quaternion worldRotation = this.transform.rotation * Quaternion.Inverse(_prevRotation);
            newPosition = ((worldRotation * (position - this.transform.position + worldDisplacement))) + this.transform.position;
        }

        #endregion

        #region Soft Contact Support

        /// <summary>
        /// Stores data for implementing Soft Contact for interaction controllers.
        /// </summary>
        [NonSerialized]
        public List<PhysicsUtility.SoftContact> _softContacts
          = new List<PhysicsUtility.SoftContact>(80);

        /// <summary>
        /// Stores data for implementing Soft Contact for interaction controllers.
        /// </summary>
        [NonSerialized]
        public Dictionary<Rigidbody, PhysicsUtility.Velocities> _softContactOriginalVelocities
          = new Dictionary<Rigidbody, PhysicsUtility.Velocities>(5);

        /// <summary>
        /// Stores data for drawing Soft Contacts for interaction controllers.
        /// </summary>
        private List<PhysicsUtility.SoftContact> _softContactsToDraw
          = new List<PhysicsUtility.SoftContact>();

        #endregion

        #region Interaction Controllers

        private void refreshInteractionControllers()
        {
            _interactionControllers.Clear();

            var tempControllers = Pool<List<InteractionController>>.Spawn();
            try
            {
                this.transform.GetComponentsInChildren<InteractionController>(false, tempControllers);
                foreach (var controller in tempControllers)
                {
                    _interactionControllers.Add(controller);
                }
            }
            finally
            {
                tempControllers.Clear();
                Pool<List<InteractionController>>.Recycle(tempControllers);
            }
        }

        #endregion

        #region Layers

        #region Automatic Layers

        protected void generateAutomaticLayers()
        {
            _interactionLayer = -1;
            _interactionNoContactLayer = -1;
            _contactBoneLayer = -1;
            for (int i = 8; i < 32; i++)
            {
                string layerName = LayerMask.LayerToName(i);
                if (string.IsNullOrEmpty(layerName))
                {
                    if (_interactionLayer == -1)
                    {
                        _interactionLayer = i;
                    }
                    else if (_interactionNoContactLayer == -1)
                    {
                        _interactionNoContactLayer = i;
                    }
                    else if (_contactBoneLayer == -1)
                    {
                        _contactBoneLayer = i;
                        break;
                    }
                }
            }

            if (_interactionLayer == -1 || _interactionNoContactLayer == -1 || _contactBoneLayer == -1)
            {
                if (Application.isPlaying)
                {
                    enabled = false;
                }
                Debug.LogError("InteractionManager Could not find enough free layers for "
                              + "auto-setup; manual setup is required.", this.gameObject);
                _autoGenerateLayers = false;
                return;
            }
        }

        private void setupAutomaticCollisionLayers()
        {
            for (int i = 0; i < 32; i++)
            {
                // Copy ignore settings from template layer
                bool shouldIgnore = Physics.GetIgnoreLayerCollision(_templateLayer, i);
                Physics.IgnoreLayerCollision(_interactionLayer, i, shouldIgnore);
                Physics.IgnoreLayerCollision(_interactionNoContactLayer, i, shouldIgnore);

                // Contact bones, generally, shouldn't collide with anything except interaction
                // layers.
                Physics.IgnoreLayerCollision(_contactBoneLayer, i, true);
            }

            // Enable interactions between the contact bones and the interaction layer.
            Physics.IgnoreLayerCollision(_contactBoneLayer, _interactionLayer, false);

            // Disable interactions between the contact bones and the no-contact layer.
            Physics.IgnoreLayerCollision(_contactBoneLayer, _interactionNoContactLayer, true);
        }

        #endregion

        #region Interaction Object Layer Tracking

        private Dictionary<SingleLayer, HashSet<IInteractionBehaviour>> _intObjInteractionLayers = new Dictionary<SingleLayer, HashSet<IInteractionBehaviour>>();
        private Dictionary<SingleLayer, HashSet<IInteractionBehaviour>> _intObjNoContactLayers = new Dictionary<SingleLayer, HashSet<IInteractionBehaviour>>();

        private int _interactionLayerMask = 0;
        /// <summary>
        /// Returns a layer mask containing all layers that might contain interaction objects.
        /// </summary>
        public int GetInteractionLayerMask()
        {
            return _interactionLayerMask;
        }

        private void refreshInteractionLayerMask()
        {
            _interactionLayerMask = 0;

            // Accumulate single-layer layer masks into the combined interaction layer mask.
            foreach (var layerObjSetPair in _intObjInteractionLayers)
            {
                // Skip any layers that may no longer have interaction objects.
                if (layerObjSetPair.Value.Count == 0) continue;

                _interactionLayerMask = layerObjSetPair.Key.layerMask | _interactionLayerMask;
            }
            foreach (var layerObjSetPair in _intObjNoContactLayers)
            {
                // Skip any layers that may no longer have interaction objects.
                if (layerObjSetPair.Value.Count == 0) continue;

                _interactionLayerMask = layerObjSetPair.Key.layerMask | _interactionLayerMask;
            }
        }

        private bool[] _contactBoneIgnoreCollisionLayers = new bool[32];
        /// <summary>
        /// Updates the contact bone layer to collide against any layers that may contain
        /// interaction objects and ignore any layers that don't.
        ///
        /// (Obviously, this ignores NoContact layers.)
        /// </summary>
        private void autoUpdateContactBoneLayerCollision()
        {
            // Make sure we ignore all layers by default!
            for (int i = 0; i < 32; i++)
            {
                _contactBoneIgnoreCollisionLayers[i] = true;
            }

            // Ignore everything except those layers that we know are at least one
            // interaction object's interaction layer.
            foreach (var layerObjSetPair in _intObjInteractionLayers)
            {
                bool ignoreLayerCollision;

                if (layerObjSetPair.Value.Count == 0)
                {
                    ignoreLayerCollision = true;
                }
                else
                {
                    ignoreLayerCollision = false;
                }

                if (layerObjSetPair.Key.layerIndex < _contactBoneIgnoreCollisionLayers.Length)
                {
                    _contactBoneIgnoreCollisionLayers[layerObjSetPair.Key.layerIndex] = ignoreLayerCollision;
                }
            }

            for (int i = 0; i < 32; i++)
            {
                Physics.IgnoreLayerCollision(contactBoneLayer.layerIndex, i,
                                              _contactBoneIgnoreCollisionLayers[i]);
            }
        }

        void IInternalInteractionManager.RefreshLayersNow()
        {
            refreshInteractionLayerMask();
        }

        void IInternalInteractionManager.NotifyIntObjAddedInteractionLayer(IInteractionBehaviour intObj, int layer, bool refreshImmediately)
        {
            if (!_intObjInteractionLayers.ContainsKey(layer))
            {
                _intObjInteractionLayers[layer] = new HashSet<IInteractionBehaviour>();
            }

            _intObjInteractionLayers[layer].Add(intObj);

            if (refreshImmediately)
            {
                refreshInteractionLayerMask();
            }
        }

        void IInternalInteractionManager.NotifyIntObjRemovedInteractionLayer(IInteractionBehaviour intObj, int layer, bool refreshImmediately)
        {
            _intObjInteractionLayers[layer].Remove(intObj);

            if (refreshImmediately)
            {
                refreshInteractionLayerMask();
            }
        }

        void IInternalInteractionManager.NotifyIntObjAddedNoContactLayer(IInteractionBehaviour intObj, int layer, bool refreshImmediately)
        {
            if (!_intObjNoContactLayers.ContainsKey(layer))
            {
                _intObjNoContactLayers[layer] = new HashSet<IInteractionBehaviour>();
            }

            _intObjNoContactLayers[layer].Add(intObj);

            if (refreshImmediately)
            {
                refreshInteractionLayerMask();
            }
        }

        void IInternalInteractionManager.NotifyIntObjRemovedNoContactLayer(IInteractionBehaviour intObj, int layer, bool refreshImmediately)
        {
            _intObjNoContactLayers[layer].Remove(intObj);

            if (refreshImmediately)
            {
                refreshInteractionLayerMask();
            }
        }

        #endregion

        #endregion

        #region Runtime Gizmos

        public void OnDrawRuntimeGizmos(RuntimeGizmoDrawer drawer)
        {
            if (_drawControllerRuntimeGizmos)
            {
                foreach (var controller in _interactionControllers)
                {
                    if (controller != null)
                    {
                        controller.OnDrawRuntimeGizmos(drawer);
                    }
                }

                foreach (PhysicsUtility.SoftContact contact in _softContactsToDraw)
                {
                    drawer.DrawSphere(contact.position, 0.01f);
                    drawer.DrawLine(contact.position, contact.position + (contact.normal * 0.02f));
                }
            }
        }

        #endregion

    }

}